#include "Matrix4.h"
#include "Vector4.h"
#include "Vector3.h"
#include <math.h>
#include <iostream>

Matrix4::Matrix4()
{
    setIdentity();
}

void  Matrix4::setMatrix(float n__11, float n__12, float n__13, float n__14,
            float n__21, float n__22, float n__23, float n__24,
            float n__31, float n__32, float n__33, float n__34,
            float n__41, float n__42, float n__43, float n__44)
{

    _11 = n__11; _12 = n__12; _13 = n__13; _14 = n__14;
    _21 = n__21; _22 = n__22; _23 = n__23; _24 = n__24;
    _31 = n__31; _32 = n__32; _33 = n__33; _34 = n__34;
    _41 = n__41; _42 = n__42; _43 = n__43; _44 = n__44;

}

void  Matrix4::setIdentity()
{
    setMatrix(1, 0, 0, 0,
              0, 1, 0, 0,
              0, 0, 1, 0,
              0, 0, 0, 1);
}

void  Matrix4::setTranslate(float x, float y, float z)
{
    setMatrix(1, 0, 0, x,
              0, 1, 0, y,
              0, 0, 1, z,
              0, 0, 0, 1);
}

void  Matrix4::setRotateX(double r)
{
    setMatrix(1, 0, 0, 0,
              0, (float)cos(r), -1 * (float)sin(r), 0,
              0, (float)sin(r), (float)cos(r), 0,
              0, 0, 0, 1);
}

void  Matrix4::setRotateY(double r)
{
    setMatrix((float)cos(r), 0, (float)sin(r), 0,
                                    0, 1, 0, 0,
              -1 * (float)sin(r), 0, (float)cos(r), 0,
                                    0, 0, 0, 1);
}

void  Matrix4::setRotateZ(double r)
{
    setMatrix((float)cos(r), -1 * (float)sin(r), 0, 0,
              (float)sin(r), (float)cos(r), 0, 0,
                               0, 0, 1, 0,
                               0, 0, 0, 1);
}

void  Matrix4::setScale(float scaleSize)
{
setMatrix(scaleSize, 0, 0, 0,
          0, scaleSize, 0, 0,
          0, 0, scaleSize, 0,
          0, 0, 0, 1);
}

Vector4  Matrix4::Transform(Vector4 inVec)
{
    float x = inVec.x;
    float y = inVec.y;
    float z = inVec.z;
    float w = inVec.w;

    float out_x = _11 * x + _12 * y + _13 * z + _14 * w;
    float out_y = _21 * x + _22 * y + _23 * z + _24 * w;
    float out_z = _31 * x + _32 * y + _33 * z + _34 * w;

    return Vector4(out_x, out_y, out_z, inVec.w);
}

Matrix4 Matrix4::mult(Matrix4 A)
{
    Matrix4 B = *this;
    Matrix4 C;

    C._11 = A._11 * B._11 + A._12*B._21 + A._13*B._31 + A._14*B._41;
    C._12 = A._11 * B._12 + A._12*B._22 + A._13*B._32 + A._14*B._42;
    C._13 = A._11 * B._13 + A._12*B._23 + A._13*B._33 + A._14*B._43;
    C._14 = A._11 * B._14 + A._12*B._24 + A._13*B._34 + A._14*B._44;

    C._21 = A._21 * B._11 + A._22*B._21 + A._23*B._31 + A._24*B._41;
    C._22 = A._21 * B._12 + A._22*B._22 + A._23*B._32 + A._24*B._42;
    C._23 = A._21 * B._13 + A._22*B._23 + A._23*B._33 + A._24*B._43;
    C._24 = A._21 * B._14 + A._22*B._24 + A._23*B._34 + A._24*B._44;

    C._31 = A._31 * B._11 + A._32*B._21 + A._33*B._31 + A._34*B._41;
    C._32 = A._31 * B._12 + A._32*B._22 + A._33*B._32 + A._34*B._42;
    C._33 = A._31 * B._13 + A._32*B._23 + A._33*B._33 + A._34*B._43;
    C._34 = A._31 * B._14 + A._32*B._24 + A._33*B._34 + A._34*B._44;

    C._41 = A._41 * B._11 + A._42*B._21 + A._43*B._31 + A._44*B._41;
    C._42 = A._41 * B._12 + A._42*B._22 + A._43*B._32 + A._44*B._42;
    C._43 = A._41 * B._13 + A._42*B._23 + A._43*B._33 + A._44*B._43;
    C._44 = A._41 * B._14 + A._42*B._24 + A._43*B._34 + A._44*B._44;

    return C;
}


std::ostream& operator<< (std::ostream& out, const Matrix4 mtx) {
    out << "Printing Matrix4: " << std::endl;
    out << mtx._11 << " " << mtx._12 << " " << mtx._13 << " " << mtx._14 << std::endl;
    out << mtx._21 << " " << mtx._22 << " " << mtx._23 << " " << mtx._24 << std::endl;
    out << mtx._31 << " " << mtx._32 << " " << mtx._33 << " " << mtx._34 << std::endl;
    out << mtx._41 << " " << mtx._42 << " " << mtx._43 << " " << mtx._44 << std::endl;
    return out;
}
